#include <cstdlib>
#include <stdio.h>
#include <math.h>

#include <windows.h>
#include <sstream>

#include "vmatrix.h"

#define PREALLOCATED_STACK_SIZE 3

VMatrix::VMatrix (void)
{
	stackSize = 1;
	preallocatedStackSize = PREALLOCATED_STACK_SIZE;
	stack = new float[ preallocatedStackSize * 16 ]; //(float*) malloc(preallocatedStackSize * sizeof(float) * 16);

	// default matrix of the stack
	LoadIdentity();

	// creating scrap matrix
	scrapMatrix = new float[16];//(float*) malloc(sizeof(float) * 16);
}

VMatrix::~VMatrix (void)
{
	//free(scrapMatrix);
	//free(stack);
	delete [] stack;
	delete [] scrapMatrix;
}

void VMatrix::Push (void)
{
	if (++stackSize > preallocatedStackSize)
	{
		// increase preallocated stack
		preallocatedStackSize = preallocatedStackSize * 2;
		float *newStack = (float*) realloc (stack, preallocatedStackSize * sizeof(float) * 16);
		if( newStack )
			stack = newStack;
		else
			int a = 123;
	}

	// duplicates current matrix
	for (int i = 0; i < 16; i++)
		stack[STACK_CURRENT + i] = stack[(stackSize-2)*16 + i];
}

void VMatrix::Pop (void)
{
	if (stackSize > 1 && --stackSize < preallocatedStackSize / 4)
	{
		// decreasing preallocated stack
		preallocatedStackSize /= 2;
		float* newStack = (float*) realloc (stack, preallocatedStackSize * sizeof(float) * 16);
		if( newStack )
			stack = newStack;
		else
			int a = 123;
	}
}

void VMatrix::LoadIdentity (void)
{
	for (int i = 0; i < 16; i++)
		stack[STACK_CURRENT + i] = (i % 5 == 0) * 1.0f;
}

void VMatrix::LoadMatrix (const float *m)
{
	for (int i = 0; i < 16; i++)
		stack[STACK_CURRENT + i] = m[i];
}

void VMatrix::MultMatrix (const float *m)
{
	eraseScrapMatrix();

	for (int i = 0; i < 16; i++)
	{
		int l = i % 4;
		int c = (i / 4) * 4;
		for (int j = 0; j < 4; j++)
			scrapMatrix[c + l] += m[c + j] * stack[STACK_CURRENT + l + (j*4)];
	}

	for (int i = 0; i < 16; i++)
		stack[STACK_CURRENT + i] = scrapMatrix[i];
}

void VMatrix::eraseScrapMatrix (void)
{
	for (int i = 0; i < 16; i++)
		scrapMatrix[i] = 0.0;
}

void VMatrix::MultVector (const float *v, float *out)
{
	for (int i = 0; i < 4; i++)
	{
		out[i] = 0.0f;
		for (int j = 0; j < 4; j++)
			out[i] += stack[STACK_CURRENT + i + j*4] * v[j]; 
	}
}

void VMatrix::Translate (float x, float y, float z)
{
	const float m[16] = {
		1.0f, 0.0f, 0.0f, 0.0f,
		0.0f, 1.0f, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f, 0.0f,
		x,    y,    z,    1.0f
	};
	MultMatrix(m);
}

void VMatrix::Scale (float x, float y, float z)
{
	const float m[16] = {
		x,	  0.0f, 0.0f, 0.0f,
		0.0f, y,    0.0f, 0.0f,
		0.0f, 0.0f, z,    0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	};
	MultMatrix(m);
}

void VMatrix::Rotate (float a, float x, float y, float z)
{
	// normalizing the vector
	normalize (&x, &y, &z);

	// turning a from degrees to radians
	a *= (3.1415926f/180.0f);

	// calculating sines and cosines
	float c = cos (a);
	float s = sin (a);

	// todo: transpor!!
	const float m[16] = {
		( x * x * (1.0f - c) ) + c,		( y * x * (1.0f - c) ) + z * s,		( z * x * (1.0f - c) ) - y * s,		0.0f,
		( x * y * (1.0f - c) ) - z * s,	( y * y * (1.0f - c) ) + c,			( z * y * (1.0f - c) ) + x * s,		0.0f,
		( x * z * (1.0f - c) ) + y * s,	( y * z * (1.0f - c) ) - x * s,		( z * z * (1.0f - c) ) + c,			0.0f,
		0.0f,							0.0f,								0.0f,								1.0f
	};
	MultMatrix (m);
}

void VMatrix::LookAt (float ex, float ey, float ez, float cx, float cy, float cz, float ux, float uy, float uz)
{
	float fx = cx - ex, fy = cy - ey, fz = cz - ez;
	normalize (&fx, &fy, &fz);
	normalize (&ux, &uy, &uz);
	
	float sx, sy, sz, wx, wy, wz;
	crossProduct (fx, fy, fz, ux, uy, uz, &sx, &sy, &sz);
	crossProduct (sx, sy, sz, fx, fy, fz, &wx, &wy, &wz);

	float m[16] = {
		sx,		wx,		-fx,	0.0f,
		sy,		wy,		-fy,	0.0f,
		sz,		wz,		-fz,	0.0f,
		0.0f,	0.0f,	0.0f,	1.0f
	};

	MultMatrix (m);
	Translate (-ex, -ey, -ez);
}

void VMatrix::Frustum (float xmin, float xmax, float ymin, float ymax, float znear, float zfar)
{
	float m[16] = {
		(2*znear)/(xmax - xmin),		0.0f,							0.0f,								0.0f,
		0.0f,							(2*znear)/(ymax - ymin),		0.0f,								0.0f,
		(xmax + xmin)/(xmax - xmin),	(ymax + ymin)/(ymax - ymin),	-(zfar + znear)/(zfar - znear),		-1.0f,
		0.0f,							0.0f,							-(2*znear*zfar)/(zfar - znear),		0.0f
	};
	MultMatrix (m);
}

void VMatrix::Perspective (float fovy, float aspect, float znear, float zfar)
{
	// converting fovy to radians
	fovy *= (3.1415926f/180.0f);

	float f = 1.0f / tan (fovy / 2.0f);
	float m[16] = {
		f/aspect,	0.0f,		0.0f,							0.0f,
		0.0f,		f,			0.0f,							0.0f,
		0.0f,		0.0f,		(zfar + znear)/(znear - zfar),	-1.0f,
		0.0f,		0.0f,		(2*znear*zfar)/(znear - zfar),	0.0f
	};
	MultMatrix (m);
}

void VMatrix::GetMatrix (float *m) const
{
	for (int i = 0; i < 16; i++)
		m[i] = stack[STACK_CURRENT + i];
}

void VMatrix::GetMatrixIT3X3 (float *mt) const
{
	// inverting the matrix
	int actualsize = 3;
	int maxsize = 3;
	float data[9];
	data[0] = Get(1,1);
	data[1] = Get(2,1);
	data[2] = Get(3,1);
	data[3] = Get(1,2);
	data[4] = Get(2,2);
	data[5] = Get(3,2);
	data[6] = Get(1,3);
	data[7] = Get(2,3);
	data[8] = Get(3,3);

    for (int i=1; i < actualsize; i++) data[i] /= data[0]; // normalize row 0
    for (int i=1; i < actualsize; i++)  { 
      for (int j=i; j < actualsize; j++)  { // do a column of L
        float sum = 0.0f;
        for (int k = 0; k < i; k++)  
            sum += data[j*maxsize+k] * data[k*maxsize+i];
        data[j*maxsize+i] -= sum;
        }
      if (i == actualsize-1) continue;
      for (int j=i+1; j < actualsize; j++)  {  // do a row of U
        float sum = 0.0f;
        for (int k = 0; k < i; k++)
            sum += data[i*maxsize+k]*data[k*maxsize+j];
        data[i*maxsize+j] = 
           (data[i*maxsize+j]-sum) / data[i*maxsize+i];
        }
      }
    for ( int i = 0; i < actualsize; i++ )  // invert L
      for ( int j = i; j < actualsize; j++ )  {
        float x = 1.0f;
        if ( i != j ) {
          x = 0.0f;
          for ( int k = i; k < j; k++ ) 
              x -= data[j*maxsize+k]*data[k*maxsize+i];
          }
        data[j*maxsize+i] = x / data[j*maxsize+j];
        }
    for ( int i = 0; i < actualsize; i++ )   // invert U
      for ( int j = i; j < actualsize; j++ )  {
        if ( i == j ) continue;
        float sum = 0.0f;
        for ( int k = i; k < j; k++ )
            sum += data[k*maxsize+j]*( (i==k) ? 1.0f : data[i*maxsize+k] );
        data[i*maxsize+j] = -sum;
        }
    for ( int i = 0; i < actualsize; i++ )   // final inversion
      for ( int j = 0; j < actualsize; j++ )  {
        float sum = 0.0f;
        for ( int k = ((i>j)?i:j); k < actualsize; k++ )  
            sum += ((j==k)?1.0f:data[j*maxsize+k])*data[k*maxsize+i];
        data[j*maxsize+i] = sum;
        }
	
	// transposing
	for(int i = 0; i < 9; i++) 
		mt[i] = data[(i % 3) * 3 + (i / 3)];
}

void VMatrix::GetMatrixIT (float *mit) const
{
	// inverting the matrix
	int actualsize = 4;
	int maxsize = 4;
	float data[16];
	GetMatrixT (data);

    for (int i=1; i < actualsize; i++) data[i] /= data[0]; // normalize row 0
    for (int i=1; i < actualsize; i++)  { 
      for (int j=i; j < actualsize; j++)  { // do a column of L
        float sum = 0.0f;
        for (int k = 0; k < i; k++)  
            sum += data[j*maxsize+k] * data[k*maxsize+i];
        data[j*maxsize+i] -= sum;
        }
      if (i == actualsize-1) continue;
      for (int j=i+1; j < actualsize; j++)  {  // do a row of U
        float sum = 0.0f;
        for (int k = 0; k < i; k++)
            sum += data[i*maxsize+k]*data[k*maxsize+j];
        data[i*maxsize+j] = 
           (data[i*maxsize+j]-sum) / data[i*maxsize+i];
        }
      }
    for ( int i = 0; i < actualsize; i++ )  // invert L
      for ( int j = i; j < actualsize; j++ )  {
        float x = 1.0f;
        if ( i != j ) {
          x = 0.0f;
          for ( int k = i; k < j; k++ ) 
              x -= data[j*maxsize+k]*data[k*maxsize+i];
          }
        data[j*maxsize+i] = x / data[j*maxsize+j];
        }
    for ( int i = 0; i < actualsize; i++ )   // invert U
      for ( int j = i; j < actualsize; j++ )  {
        if ( i == j ) continue;
        float sum = 0.0;
        for ( int k = i; k < j; k++ )
            sum += data[k*maxsize+j]*( (i==k) ? 1.0f : data[i*maxsize+k] );
        data[i*maxsize+j] = -sum;
        }
    for ( int i = 0; i < actualsize; i++ )   // final inversion
      for ( int j = 0; j < actualsize; j++ )  {
        float sum = 0.0f;
        for ( int k = ((i>j)?i:j); k < actualsize; k++ )  
            sum += ((j==k)?1.0f:data[j*maxsize+k])*data[k*maxsize+i];
        data[j*maxsize+i] = sum;
        }
	
	// transposing
	for(int i = 0; i < 16; i++) 
		mit[i] = data[(i % 4) * 4 + (i / 4)];
}

void VMatrix::GetMatrixT (float *m) const
{
	// transposing
	for(int i = 0; i < 16; i++)
		m[i] = stack[STACK_CURRENT + ((i % 4) * 4 + (i / 4))];
}

// move away from here?
void VMatrix::normalize (float *x, float *y, float *z)
{
	float norm = sqrt ( (*x)*(*x) + (*y)*(*y) + (*z)*(*z));
	*x /= norm;
	*y /= norm;
	*z /= norm;
}

void VMatrix::crossProduct(float a1, float a2, float a3, float b1, float b2, float b3, float *c1, float *c2, float *c3)
{
	*c1 = (a2*b3) - (a3*b2);
	*c2 = (a3*b1) - (a1*b3);
	*c3 = (a1*b2) - (a2*b1);
}

float VMatrix::Get( int l, int c ) const
{
	return stack[STACK_CURRENT + ( (c-1) * 4 ) + (l-1) ];
}

void VMatrix::Print()
{
	std::stringstream sstr;
	sstr << "| " << stack[STACK_CURRENT + 0] << "\t" << stack[STACK_CURRENT + 4] << "\t" << stack[STACK_CURRENT + 8] << "\t" << stack[STACK_CURRENT + 12] << " |\n";
	sstr << "| " << stack[STACK_CURRENT + 1] << "\t" << stack[STACK_CURRENT + 5] << "\t" << stack[STACK_CURRENT + 9] << "\t" << stack[STACK_CURRENT + 13] << " |\n";
	sstr << "| " << stack[STACK_CURRENT + 2] << "\t" << stack[STACK_CURRENT + 6] << "\t" << stack[STACK_CURRENT + 10] << "\t" << stack[STACK_CURRENT + 14] << " |\n";
	sstr << "| " << stack[STACK_CURRENT + 3] << "\t" << stack[STACK_CURRENT + 7] << "\t" << stack[STACK_CURRENT + 11] << "\t" << stack[STACK_CURRENT + 15] << " |\n\n";
	OutputDebugString( (LPCTSTR) sstr.str().data() );
}

void VMatrix::ScalarMult( float a )
{
	for (int i = 0; i < 16; i++)
		stack[STACK_CURRENT + i] *= a;
}

void VMatrix::ScalarMult3x3( float a )
{
	for (int i = 0; i < 12; i++)
		stack[STACK_CURRENT + i] *= a;
}

void VMatrix::Sum( const VMatrix& mat )
{
	for (int i = 0; i < 16; i++)
	{
		int l = ( i % 4 ) + 1;
		int c = ( i / 4 ) + 1;
		stack[STACK_CURRENT + i] += mat.Get( l, c );
	}
}

float VMatrix::Determinant()
{
	float** a = (float**) malloc( 4 * sizeof( float* ) );
	for( int i = 0; i < 4; i++ )
	{ 
		a[i] = (float*) malloc( 4 * sizeof( float* ) );
	}

	for (int i = 0; i < 16; i++)
	{
		int l = i % 4;
		int c = (i / 4);
		a[l][c] = stack[STACK_CURRENT + i];
	}

	float det = internalDet( a, 4 );

	for( int i = 0; i < 4; i++ )
		free( a[i] );
	free( a );

	return det;
}

float VMatrix::internalDet( float**a, int n )
{
   int i,j,j1,j2;
   float det = 0;
   float **m = NULL;

   if (n < 1) { /* Error */

   } else if (n == 1) { /* Shouldn't get used */
      det = a[0][0];
   } else if (n == 2) {
      det = a[0][0] * a[1][1] - a[1][0] * a[0][1];
   } else {
      det = 0;
      for (j1=0;j1<n;j1++) {
         m = (float**) malloc((n-1)*sizeof(float *));
         for (i=0;i<n-1;i++)
            m[i] = (float*) malloc((n-1)*sizeof(float));
         for (i=1;i<n;i++) {
            j2 = 0;
            for (j=0;j<n;j++) {
               if (j == j1)
                  continue;
               m[i-1][j2] = a[i][j];
               j2++;
            }
         }
         det += pow(-1.0f,j1+2.0f) * a[0][j1] * internalDet(m,n-1);
         for (i=0;i<n-1;i++)
            free(m[i]);
         free(m);
      }
   }
   return(det);
}

float VMatrix::Norm()
{
	float norm = 0.0f;

	for( int i = 0; i < 16; i++ )
		norm += stack[ STACK_CURRENT + i ] * stack[ STACK_CURRENT + i ];

	return sqrt( norm );
}

void VMatrix::Set( int l, int c, float value )
{
	stack[STACK_CURRENT + ( (c-1) * 4 ) + (l-1) ] = value;
}
